#pragma once

#include <openvr.h>
#include <cmath>


namespace utils {

inline vr::HmdMatrix34_t& initRotationMatrix(vr::HmdMatrix34_t& matrix, unsigned axisId, float angle) {
	switch (axisId) {
		case 0: // x-axis
			matrix.m[0][0] = 1.0f;
			matrix.m[0][1] = 0.0f;
			matrix.m[0][2] = 0.0f;
			matrix.m[0][3] = 0.0f;
			matrix.m[1][0] = 0.0f;
			matrix.m[1][1] = std::cos(angle);
			matrix.m[1][2] = -std::sin(angle);
			matrix.m[1][3] = 0.0f;
			matrix.m[2][0] = 0.0f;
			matrix.m[2][1] = std::sin(angle);
			matrix.m[2][2] = std::cos(angle);
			matrix.m[2][3] = 0.0f;
			break;
		case 1: // y-axis
			matrix.m[0][0] = std::cos(angle);
			matrix.m[0][1] = 0.0f;
			matrix.m[0][2] = std::sin(angle);
			matrix.m[0][3] = 0.0f;
			matrix.m[1][0] = 0.0f;
			matrix.m[1][1] = 1.0f;
			matrix.m[1][2] = 0.0f;
			matrix.m[1][3] = 0.0f;
			matrix.m[2][0] = -std::sin(angle);
			matrix.m[2][1] = 0.0f;
			matrix.m[2][2] = std::cos(angle);
			matrix.m[2][3] = 0.0f;
			break;
		case 2: // z-axis
			matrix.m[0][0] = std::cos(angle);
			matrix.m[0][1] = -std::sin(angle);
			matrix.m[0][2] = 0.0f;
			matrix.m[0][3] = 0.0f;
			matrix.m[1][0] = std::sin(angle);
			matrix.m[1][1] = std::cos(angle);
			matrix.m[1][2] = 0.0f;
			matrix.m[1][3] = 0.0f;
			matrix.m[2][0] = 0.0f;
			matrix.m[2][1] = 0.0f;
			matrix.m[2][2] = 1.0f;
			matrix.m[2][3] = 0.0f;
			break;
		default:
			break;
	}
	return matrix;
}


inline vr::HmdMatrix34_t& matMul33(vr::HmdMatrix34_t& result, const vr::HmdMatrix34_t& a, const vr::HmdMatrix34_t& b) {
	for (unsigned i = 0; i < 3; i++) {
		for (unsigned j = 0; j < 3; j++) {
			result.m[i][j] = 0.0f;
			for (unsigned k = 0; k < 3; k++) {
				result.m[i][j] += a.m[i][k] * b.m[k][j];
			}
		}
	}
	return result;
}


inline vr::HmdVector3_t& matMul33(vr::HmdVector3_t& result, const vr::HmdMatrix34_t& a, const vr::HmdVector3_t& b) {
	for (unsigned i = 0; i < 3; i++) {
		result.v[i] = 0.0f;
		for (unsigned k = 0; k < 3; k++) {
			result.v[i] += a.m[i][k] * b.v[k];
		};
	}
	return result;
}


inline vr::HmdVector3_t& matMul33(vr::HmdVector3_t& result, const vr::HmdVector3_t& a, const vr::HmdMatrix34_t& b) {
	for (unsigned i = 0; i < 3; i++) {
		result.v[i] = 0.0f;
		for (unsigned k = 0; k < 3; k++) {
			result.v[i] +=  a.v[k] * b.m[k][i];
		};
	}
	return result;
}



} // end namespace utils

